package pack.help.realRobot.uC;
import pack.help.RobotCommand;
import pack.help.realRobot.Motor;

public class UC {

	private TimerCnt timerL;
	private TimerCnt timerR;
	
	private Port portL;
	private Port portR;

	private Buffer buffer;
	
	private Flag flag;
	private Flag timerLFlag;
	private Flag timerRFlag;
	
	public UC(Motor MotorL, Motor MotorR){
		
		timerL = new TimerCnt(this,WhichSide.LEFT);
		timerR = new TimerCnt(this,WhichSide.RIGHT);
		
		portL = new Port(MotorL);
		portR = new Port(MotorR);
		
		buffer = new Buffer();
		
		flag = Flag.READY;
		timerLFlag = Flag.READY;
		timerRFlag = Flag.READY;
	}
	
	public void runCmd(RobotCommand cmd){
		// SET TIMERS, SHIFT BUFFER
		switch (buffer.getBufferedCmds()){
		case 0:
			buffer.addCommand(cmd);
			startTimers(cmd);
			break;
		case 1:
			flag = Flag.BUSY;
			buffer.addCommand(cmd);
			
			timerL.setNextSpeed(cmd.getSpeedL());
			timerR.setNextSpeed(cmd.getSpeedR());
			
			if (cmd.getDirL() != buffer.getCurrent().getDirL()){
				timerL.setNextSpeed(TimerCnt.maxValue);
			}
			
			if (cmd.getDirR() != buffer.getCurrent().getDirR()){
				timerR.setNextSpeed(TimerCnt.maxValue);
			}
			
			if (cmd.getSpeedL() == 0){
				timerL.setNextSpeed(255);
			}
			
			if (cmd.getSpeedR() == 0){
				timerR.setNextSpeed(255);
			}
			
			
			break;
		default: 
			break;
		}
		
		
	}
	
	public void invertPort(WhichSide which){
		switch (which){
		case LEFT :
			portL.invert();
			break;
		case RIGHT:
			portR.invert();
			break;
		}
	}
	
	public Port getPort(WhichSide which){
		switch (which){
		case LEFT :
			return portL;
		case RIGHT:
			return portR;
		default:
			return portL;
		}
	}
	
	public Flag pingCmd(){
		return flag;
	}
	
	public void stopCmd(){
		timerL.stop();
		timerR.stop();
		buffer.reset();
	}
	
	public void timerEnded(WhichSide which) {
		// CHECK FOR OTHER TIMER THEN SET FLAG ACCORDINGLY
		switch (which){
		case LEFT :
			timerLFlag = Flag.READY;
			break;
		case RIGHT:
			timerRFlag = Flag.READY;
			break;
		}
		
		if (timerRFlag == Flag.READY && timerLFlag == Flag.READY){
			commandOver();
		}
	}
	
	public void commandOver(){
		switch (buffer.getBufferedCmds()){
		case 1:
			buffer.shift();
			break;
		case 2:
			buffer.shift();
			startTimers(buffer.getCurrent());
			break;
		default:
			break;
		}
		flag = Flag.READY;
	}
	
	private void startTimers(RobotCommand cmd){
		timerLFlag = Flag.BUSY;
		timerRFlag = Flag.BUSY;
		timerL.start(cmd.getStepsL(), cmd.getSpeedL());
		timerR.start(cmd.getStepsR(), cmd.getSpeedR());
	}
	
	
	public enum Flag{
		READY, BUSY;
	}
	
	public enum WhichSide{
		LEFT, RIGHT;
	}
}
